#pragma ident "$Id: XY. CAO 2017-08-15 $"

/**
* @file CorrectPhaseBias.hpp
* Class to correct observables with satellite upds.
*/

//============================================================================
//
//  shjzhang, 2013/03/01, create
//
//============================================================================


#include "CorrectPhaseBias.hpp"
#include "constants.hpp"

#define debug 1

namespace gpstk
{
	// Index initially assigned to this class
	int CorrectPhaseBias::classIndex = 4900000;


	// Returns an index identifying this object.
	int CorrectPhaseBias::getIndex() const
	{ return index; }


	// Returns a string identifying this object.
	std::string CorrectPhaseBias::getClassName() const
	{ return "CorrectPhaseBias"; }


	/* Returns a satTypeValueMap object, adding the new data generated
	*  when calling this object.
	*
	* @param time      Epoch corresponding to the data.
	* @param gData     Data object holding the data.
	*/
	satTypeValueMap& CorrectPhaseBias::Process( const CommonTime& time,
		                                     satTypeValueMap& gData )
		noexcept(false)
	{
		try
		{
			SatIDSet satRejectedSet;

			// Loop through all the satellites
			for (satTypeValueMap::iterator stv = gData.begin(); 
				stv != gData.end(); 
				++stv)
			{
                TypeIDSet typeSetRejected;

				// Warning:
				// you should check the unit of the UPDs in the input file.
				// if it is cycles, then convert it to meters firstly, 
				// or if it is meters, you can use it directly.
                typeValueMap wlBiasData;
                try
                {
                    // Get satellite upds from 'RinexUPDStore'
                    wlBiasData = (*pWLBiasStore).getValue( (*stv).first, time );

                    if(debug)
                    {
                        for(auto bd: wlBiasData)
                        {
                            cout << (*stv).first << " :"
                                 << bd.first << " :"
                                 << bd.second << endl;;
                        }
                    }
                }
                catch(InvalidRequest& e)
                {
                    // If some problem appears, then schedule this satellite
                    // for removal
                    if(debug)
                    cerr << "WL upd can't be found for sat:"<< (*stv).first << endl;
                    satRejectedSet.insert( (*stv).first );
                    continue;    // Skip this SV if problems arise
                }

                typeValueMap lcBiasData;
                try
                {
                    lcBiasData = (*pLCBiasStore).getValue( (*stv).first, time );

                    if(debug)
                    {
                        for(auto bd: lcBiasData)
                        {
                            cout << " sat: "<< (*stv).first
                                 << " epoch: " << time
                                 << " var:" << bd.first
                                 << " bias:" << bd.second
                                 << endl;
                        }
                    }
                }
                catch(InvalidRequest& e)
                {
                    // If some problem appears, then schedule this satellite
                    // for removal
                    if(debug)
                    cerr << "LC upd can't be found for sat:"<< (*stv).first << endl;
                    satRejectedSet.insert( (*stv).first );
                    continue;    // Skip this SV if problems arise
                }



				SatID tempSat = (*stv).first;			
				if( tempSat.system == SatelliteSystem::GPS )
				{
                    try
                    {
                        // unit is cycle
                        double updBWL12 = wlBiasData.getValue(TypeID::BWL12);
                        double updBLC12 = lcBiasData.getValue(TypeID::BLC12);

                        if(debug)
                        {
                            cout << "tempSat:" << tempSat << endl;
                            cout << "updBWL12" << updBWL12 << endl;;
                            cout << "updBLC12" << updBLC12 << endl;;
                        }

                        double f1, f2;
                        f1 = L1_FREQ_GPS;
                        f2 = L2_FREQ_GPS;

					    // Get updSatL1/updSatL2, unit:cycles
					    double updBL1 = updBLC12 - f2/(f1-f2)*updBWL12;
					    double updBL2 = updBLC12 - f1/(f1-f2)*updBWL12;

                        if(debug)
                        cout << updBL1 << ":" << updBL2 << endl;

                        // convert unit from cycles to meters
					    (*stv).second[TypeID::updL1] = L1_WAVELENGTH_GPS*updBL1;
					    (*stv).second[TypeID::updL2] = L2_WAVELENGTH_GPS*updBL2;

                    }
                    catch(Exception& e)
                    {
                        if(debug)
                        cerr << e << endl;
                        // If some value is missing, then schedule this satellite
                        // for removal
                        satRejectedSet.insert( tempSat );
                        continue;
                    }

                    try
                    {
                        // unit is cycle
                        double updBWL15 = wlBiasData.getValue(TypeID::BWL15);
                        double updBLC15 = lcBiasData.getValue(TypeID::BLC15);

                        double f1, f5;
                        f1 = L1_FREQ_GPS;
                        f5 = L5_FREQ_GPS;

					    // Get updSatL1/updSatL2, unit:cycles
					    double updBL5 = updBLC15 - f1/(f1-f5)*updBWL15;

                        // convert unit from cycles to meters
                        (*stv).second[TypeID::updL5] = L5_WAVELENGTH_GPS*updBL5;

                    }
                    catch(Exception& e)
                    {
                        // here, only remove the type from current satellite.
                        typeSetRejected.insert(TypeID::MW15);
                        typeSetRejected.insert(TypeID::LC15);
                        typeSetRejected.insert(TypeID::L5);

                        continue;
                    }

				}
				else if( tempSat.system == SatelliteSystem::Galileo )
				{
                    // to be determined
				}
				else if( tempSat.system == SatelliteSystem::BDS)
				{
                    // to be determined
				}	
				else if( tempSat.system == SatelliteSystem::GLONASS)
				{
/*
					int k(0);
					try
					{
						// Get frequency number 
						k = (*stv).second[TypeID::FreqNo];

					}
					catch(InvalidRequest& e)
					{
						// If some problem appears, then schedule this satellite
						// for removal. Thiis may be caused GLONASS not processed
						satRejectedSet.insert( (*stv).first );

						continue;    // Skip this SV if problems arise

					}

					// Frequency for this satellite
					double GLO_L1_FRE = L1_FREQ_GLO + L1_FREQ_STEP_GLO*k;
					double GLO_L2_FRE = L2_FREQ_GLO + L2_FREQ_STEP_GLO*k;
					double L1_WAVE_GLO = C_MPS/GLO_L1_FRE;
					double L2_WAVE_GLO = C_MPS/GLO_L2_FRE;
					double WL_WAVE_GLO = C_MPS/(GLO_L1_FRE - GLO_L2_FRE);
					double NL_WAVE_GLO = C_MPS/(GLO_L1_FRE + GLO_L2_FRE);

					// Change unit from meter to cycles
					updSatWL = satUPD.updSatMW/WL_WAVE_GLO;
					updSatLC = satUPD.updSatLC/NL_WAVE_GLO;

					// Get updSatL1/updSatL2, unit:cycles
					updSatL1 = updSatLC - GLO_L2_FRE/(GLO_L1_FRE-GLO_L2_FRE)*updSatWL;
					updSatL2 = updSatLC - GLO_L1_FRE/(GLO_L1_FRE-GLO_L2_FRE)*updSatWL;

					// Convert from cycles to meters
					updSatL1 = updSatL1 * L1_WAVE_GLO;
					updSatL2 = updSatL2 * L2_WAVE_GLO;

					(*stv).second[TypeID::updSatL1] = updSatL1;
					(*stv).second[TypeID::updSatL2] = updSatL2;
*/
				}	


			}  // End of 'for (stv = gData.begin(); stv != gData.end(); ++stv)'

			// Remove satellites with missing data
			gData.removeSatID(satRejectedSet);

			// Return
			return gData;

		}
		catch(Exception& u)
		{

			// Throw an exception if something unexpected happens
			ProcessingException e( getClassName() + ":"
				+ StringUtils::asString( getIndex() ) + ":"
				+ u.what() );

			GPSTK_THROW(e);

		}

	}  // End of method 'CorrectPhaseBias::Process()'

}

